#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S2,     HTSMUX,         sensorI2CCustom)
#pragma config(Sensor, S3,     HTSPB,          sensorI2CCustom9V)
#pragma config(Motor,  mtr_S1_C1_1,     left,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     right,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    servo2,               tServoStandard)

//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//#pragma config(Sensor, S2,     ,               sensorI2CMuxController)

#include "geary-mux-stuff.c"

//////////////////////////////////////////////////////////////////////////////
//
//      MAIN
//
/////////////////////////////////////////////////////////////////////////////


task
main ()
{
  wait1Msec(500);
  VerifyMux();

  // position stalk and init protoboard for lights
  OrientStalkServo();
  ExtendLightServo();
  RetractLightServo();
  InitProtoboardLights();

  // begin calculating heading
  StartTask(getHeading);
  wait1Msec(500);

  float startHeading = currHeading;
  while (true) {
    PointStraightByGyro(startHeading);
		}

  StopAllTasks ();
}
